/*=+--+=#=+--     Unmanned Aerial System Management Software      --+=#=+--+=#*\
|          Copyright (C) 2011 Regents of the University of Colorado.           |
|                             All Rights Reserved.                             |

     This program is free software: you can redistribute it and/or modify
     it under the terms of the GNU General Public License version 2 as
     published by the Free Software Foundation.

     This program is distributed in the hope that it will be useful,
     but WITHOUT ANY WARRANTY; without even the implied warranty of
     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
     GNU General Public License for more details.

     You should have received a copy of the GNU General Public License
     along with this program.  If not, see <http://www.gnu.org/licenses/>.

            Jack Elston                       Cory Dixon                        
|           elstonj@colorado.edu              dixonc@colorado.edu              |
|                                                                              |
\*=+--+=#=+--                 --+=#=+--+=#=+--                    --+=#=+--+=#*/

/***********************************************************************
 *                      
 * FILENAME:
 * quaternion.h
 *
 * PURPOSE:
 *  To implement any needed quaternion functions for transormations.
 *  Quaternions can be read form and saved to a file.
 *
 * CREATED:
 * 02/2001 by Cory Dixon
 *
 * LAST MODIFIED:
 * $Date: 2005/07/13 21:00:06 $
 * $Revision: 1.3 $
 * $Author: dixonc $
 ***********************************************************************/ 

#ifndef __cplusplus
#	error Must use C++ for the type Quaternion.
#endif

#ifndef _QUATERNION_H
#define _QUATERNION_H

#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <iostream>
#include <iomanip.h>
#include <fstream.h>
#include <string.h>
#include "matrix.h"

#define RAD2DEG (57.2957795131)
#define DEG2RAD (0.01745329252)

#ifndef PI
# define PI            3.14159265358979323846  /* pi */
#endif

//////////////////////////////////////////////////////////////////////
//
//////////////////////////////////////////////////////////////////////
class Quaternion
{

	// friend functions
	friend ostream& operator << (ostream& ostrm, Quaternion& q){
		ostrm << " [ " 
			<< q(0) << " " 
			<< q(1) << " " 
			<< q(2) << " " 
			<< q(3) << " ] ";

		// reset output format
		return ostrm;
	}

	friend istream& operator >> (istream& istrm, Quaternion& q){
		istrm >> q(0)>> q(1) >> q(2) >> q(3);	
		return istrm;
	}
	friend Quaternion operator * (const float& no, const Quaternion& q){
		return q*no;
	}

	public:

	const static float X_VEC[4];
	const static float Y_VEC[4];
	const static float Z_VEC[4];	
	// Constructors
	Quaternion ( ){
		Q[0] = 0.0; Q[1] = 0.0; Q[2] = 0.0; Q[3] = 0.0;
	}
	// specify a 3-2-1 sequence ( yaw, pitch, roll)
	Quaternion ( const float q[4] ){
		set(q);
	}
	Quaternion ( const Matrix &ypr ){
		// set yaw pitch roll
		if( ypr.NumRow() == 3 || ypr.NumCol() == 3) 
			set(ypr(0),ypr(1),ypr(2));
		// set individual elements
		else if( ypr.NumRow() == 4 || ypr.NumCol() == 4 ) {
			Q[0] = ypr(0); Q[1] = ypr(1); 
			Q[2] = ypr(2); Q[3] = ypr(3);
		}
	}
	Quaternion ( float y, float p, float r){
		set(y,p,r);
	}
	// specify the four elements
	Quaternion ( float q1, float q2, float q3, float q4){
		Q[0] = q1; Q[1] = q2; Q[2] = q3; Q[3] = q4;
	}
	Quaternion ( float angle, Matrix &xyz){
		set(angle,xyz(0), xyz(1),xyz(2));
	}

	// Destructor
	~Quaternion (){
		;
	}

	// Assignment operators
	Quaternion& operator = (const Quaternion& q){
		for(register int i=0; i < 4; i++)
			Q[i] = q(i);

		return *this;
	}

	// equality operators
	bool operator == (const Quaternion& q) const{
		return ( Q[0] == q(0) && Q[1] == q(1) && Q[2] == q(2) && Q[3] == q(3) );
	}
	bool operator != (const Quaternion& q) const{
		return !( Q[0] == q(0) && Q[1] == q(1) && Q[2] == q(2) && Q[3] == q(3) );
	}

	// Subscript operator
	// get
	float operator () (size_t ind) const{
		return Q[ind];
	}
	// set
	float& operator () (size_t ind) {
		return Q[ind];	
	}

	// unary member operator function
	Quaternion operator - () const{	// negation
		return Quaternion( -Q[0], -Q[1], -Q[2], -Q[3]);
	}
	Quaternion operator ! () const{ 	// inverse operator
		Quaternion temp;
		float n=0;
		if( ( n = this->norm() ) > 0)
			temp = *this / n;
		else
			temp.null();

		return ~temp;
	}
	Quaternion operator ~ () const{	// conjigate operator
		Quaternion temp;
		temp(0) = -Q[0];
		temp(1) = -Q[1];
		temp(2) = -Q[2];
		temp(3) = Q[3];
		return temp ;
	}

	// binary operators
	Quaternion operator + (const Quaternion& q) const{ 
		Quaternion temp;
		temp(0) = Q[0] + q(0);
		temp(1) = Q[1] + q(1);
		temp(2) = Q[2] + q(2);
		temp(3) = Q[3] + q(3);
		return temp;
	}
	Quaternion operator - (const Quaternion& q) const{
		Quaternion temp;
		temp(0) = Q[0] - q(0);
		temp(1) = Q[1] - q(1);
		temp(2) = Q[2] - q(2);
		temp(3) = Q[3] - q(3);
		return temp;
	}
	// please see page 320 of Space Vehicles Dynamics & Control
	// for q' = Q*q;
	Quaternion operator * (const Quaternion& q) const{
		Quaternion temp;
		temp(0) =  Q[3]*q(0) - Q[2]*q(1) + Q[1]*q(2) + Q[0]*q(3);
		temp(1) =  Q[2]*q(0) + Q[3]*q(1) - Q[0]*q(2) + Q[1]*q(3);
		temp(2) = -Q[1]*q(0) + Q[0]*q(1) + Q[3]*q(2) + Q[2]*q(3);
		temp(3) = -Q[0]*q(0) - Q[1]*q(1) - Q[2]*q(2) + Q[3]*q(3);

		//temp(0) =  Q[0]*q(3) + Q[1]*q(2) - Q[2]*q(1) + Q[3]*q(0);
		//temp(1) = -Q[0]*q(2) + Q[1]*q(3) + Q[2]*q(0) + Q[3]*q(1);
		//temp(2) =  Q[0]*q(1) - Q[1]*q(0) + Q[2]*q(3) + Q[3]*q(2);
		//temp(3) = -Q[0]*q(0) - Q[1]*q(1) - Q[2]*q(2) + Q[3]*q(3);

		//temp(0) =  q(3)*Q[0] - q(2)*Q[1] + q(1)*Q[2] + q(0)*Q[3];
		//temp(1) =  q(2)*Q[0] + q(3)*Q[1] - q(0)*Q[2] + q(1)*Q[3];
		//temp(2) = -q(1)*Q[0] + q(0)*Q[1] + q(3)*Q[2] + q(2)*Q[3];
		//temp(3) = -q(0)*Q[0] - q(1)*Q[1] - q(2)*Q[2] + q(3)*Q[3];

		//temp(0) =  q(3)*Q[0] + q(2)*Q[1] - q(1)*Q[2] + q(0)*Q[3];
		//temp(1) = -q(2)*Q[0] + q(3)*Q[1] + q(0)*Q[2] + q(1)*Q[3];
		//temp(2) =  q(1)*Q[0] - q(0)*Q[1] + q(3)*Q[2] + q(2)*Q[3];
		//temp(3) = -q(0)*Q[0] - q(1)*Q[1] - q(2)*Q[2] + q(3)*Q[3];
		return temp;
	}
	Quaternion operator * (const float& no) const{
		Quaternion temp;
		for(register int i=0; i < 4; i++)
			temp(i) = Q[i] * no;
		return temp;
	}
	Quaternion operator / (const float& no) const{
		Quaternion temp;
		for(register int i=0; i < 4; i++)
			temp(i) = Q[i] / no;
		return temp;
	}

	// Miscellaneous Utility methods
	void null (){		// make empty
		Q[0] = 0 ; Q[1] = 0 ; Q[2] = 0 ; Q[3] = 0;
	}
	Quaternion& unit () {		// make unit identity 
		float n=0;
		if( ( n = this->norm() ) > 0)
			*this = *this / n;
		return *this;
	}

	// math functions
	Quaternion inv () const {	// Inverse 
		return !(*this);
	}
	Quaternion conj () const {	// conjigate 
		return ~(*this);
	}
	float norm () const{		// magnitude of elements
		return sqrt( Q[0]*Q[0]  + Q[1]*Q[1] + Q[2]*Q[2] + Q[3]*Q[3] );
	}
	float sum() const{			// sum of elements
		return (Q[0]  + Q[1] + Q[2] + Q[3]);
	}


	// type of quaternion 
	bool isNull () const{		// if is empty
		return ( 0 == Q[0] && 0 == Q[1] && 0 == Q[2] && 0 == Q[3]);
	}

	// element set get
	// from 3-2-1 sequence of yaw pitch roll 
	void set(float y, float p, float r) {
		fromEuler(y, p, r);
	}
	void set(const float q[4]) {
		Q[0] = q[0]; Q[1] =  q[1]; Q[2] = q[2]; Q[3] = q[3] ;
	}
	void set(Matrix & ypr) {
		fromEuler(ypr(0), ypr(1), ypr(2));
	}

	// from angle about axis (not like constructor)
	void set(float angle, float x, float y, float z){
		fromAngleAxis(angle, x, y, z );
	}
	void set(float angle, float xyz[3]){
		fromAngleAxis(angle, xyz[0], xyz[1], xyz[2] );
	}
	void set(float angle, Matrix &xyz){
		fromAngleAxis(angle, xyz(0), xyz(1), xyz(2) );
	}

	//
	void toAngleAxis(float &angle, Matrix &axis ) const { 
		float a = (float) acos ( Q[3] ) ;

		angle = a * 2.0 ;

		axis.SetSize(3,1);
		axis(0,0) = Q[0]; axis(1,0) = Q[1]; axis(2,0) = Q[2];

		axis.Unit(); 
		/*
			cout << "axis - \n" << axis 
			<< " axis(0)=" << axis(0) 
			<< " axis(1)=" << axis(1) 
			<< " axis(2)=" << axis(2) << endl;
			*/
	}
	void toAngleAxis(float &angle, float &x, float &y, float &z ) const {
		float a = (float) acos ( Q[3] ) ;
		float s = (float) sin  ( a ) ;

		angle = a * 2.0 ;

		if ( s == 0.0 ) { 
			x = 0.0; y = 0.0; z = 1.0;
		}
		else {
			x = Q[0] / s; y = Q[1] / s; z = Q[2] / s;
		}
	}

	void fromAngleAxis(float angle, float x, float y, float z ) {
		float temp_angle = angle / 2.0 ;
		float s = (float) sin ( temp_angle ) ;

		float mag = sqrt( x*x + y*y + z*z );
		x = x/mag * s;
		y = y/mag * s;
		z = z/mag * s;

		Q[0] = x; Q[1] = y; Q[2] = z;
		Q[3] = (float) cos ( temp_angle ) ;
	}

	// file functions
	bool fillFromFile(char *file){
		ifstream inputFile(file, ios::in);
		if( !inputFile )  {
			perror("Quaternion::fillMatrixFromFile() could not open file");
			return false;
		} else {
			if( !(inputFile >> *this ) ) {
				inputFile.close();
				return false;
			}

			inputFile.close();
			return true;
		}
	}
	bool saveToFile(char *file){
		ofstream outputFile(file, ios::out);
		if( !outputFile )  {
			perror("Quaternion::saveMatrixToFile() could not open file");
			return false;
		} else {
			outputFile << *this;

			outputFile.close();
			return true;
		}
	}

	void fromMatrix(Matrix const &mat) {
		float tr, s, q[4] ;
		int   i, j, k ;

		int nxt[3] = {1, 2, 0};

		tr = mat.Trace();

		// check the diagonal
		if (tr > 0 ) {
			s = (float) sqrt (tr + 1.0);
			Q[3] = s / 2.0;
			s = 0.5 / s;
			Q[0] = (mat(1,2) - mat(2,1)) * s;
			Q[1] = (mat(2,0) - mat(0,2)) * s;
			Q[2] = (mat(0,1) - mat(1,0)) * s;
		} else {             
			// diagonal is negative
			i = 0;
			if (mat(1,1) > mat(0,0)) 
				i = 1;
			if (mat(2,2) > mat(i,i)) 
				i = 2;

			j = nxt[i];
			k = nxt[j];
			s = (float) sqrt ((mat(i,i) - (mat(j,j) + mat(k,k))) + 1.0);
			q[i] = s * 0.5;

			if (s != 0) 
				s = 0.5 / s;

			q[3] = (mat(j,k) - mat(k,j)) * s;
			q[j] = (mat(i,j) + mat(j,i)) * s;
			q[k] = (mat(i,k) + mat(k,i)) * s;

			Q[0] = q[0];
			Q[1] = q[1];
			Q[2] = q[2];
			Q[3] = q[3];
		}

		// yields the inverse rotation, so:
		Q[3] = -Q[3];
	}
	void toMatrix ( Matrix &mat ) const {
		float q1_2 = Q[0]*Q[0];
		float q2_2 = Q[1]*Q[1];
		float q3_2 = Q[2]*Q[2];
		float q4_2 = Q[3]*Q[3];

		float q1q2 = Q[0]*Q[1];
		float q1q3 = Q[0]*Q[2];
		float q1q4 = -Q[0]*Q[3];	

		float q2q3 = Q[1]*Q[2];
		float q2q4 = -Q[1]*Q[3];

		float q3q4 = -Q[2]*Q[3];

		float val[] = {	
			q1_2 - q2_2 - q3_2 + q4_2, 2.0*(q1q2 + q3q4), 2.0*(q1q3 - q2q4),
			2.0*(q1q2 - q3q4), -q1_2 + q2_2 - q3_2 + q4_2, 2.0*(q2q3 + q1q4),
			2.0*(q1q3 + q2q4),	2.0*(q2q3 - q1q4),	-q1_2 - q2_2 + q3_2 + q4_2};

		//mat.SetSize(3,3);
		mat = Matrix(3,3,val);
	}

	void fromEuler( float y, float p, float r) {
		float cr, cp, cy, sr, sp, sy;

		// calculate trig identities
		cr = (float) cos(r/2.0);
		cp = (float) cos(p/2.0);
		cy = (float) cos(y/2.0);

		sr = (float) sin(r/2.0);
		sp = (float) sin(p/2.0);
		sy = (float) sin(y/2.0);

		Q[3] = cr * cp * cy + sr * sp * sy;
		Q[0] = sr * cp * cy - cr * sp * sy;
		Q[1] = cr * sp * cy + sr * cp * sy;
		Q[2] = cr * cp * sy - sr * sp * cy;
	}
	void fromEuler( Matrix const & ypr ) {
		fromEuler( ypr(0), ypr(1), ypr(2));
	}
	void toEuler( Matrix &ypr) const {
		ypr.SetSize(3,1);
		toEuler(ypr(0), ypr(1),ypr(2));
	}
	void toEuler( float &y, float &p, float &r) const {
		Matrix matrix(3,3);
		float cx,sx;
		float cy,sy,yr;
		float cz,sz;

		// CONVERT QUATERNION TO MATRIX 
		this->toMatrix(matrix);
		//cout << " matrix: " << endl << matrix ;

		sy = -matrix(2,0);
		cy = (float)sqrt(1.0 - (sy * sy));
		yr = (float)atan2(sy,cy);
		p = yr; // * RAD2DEG ;

		// AVOID DIVIDE BY ZERO ERROR ONLY WHERE Y= +-90 or +-270 
		// NOT CHECKING cy BECAUSE OF PRECISION ERRORS
		if (sy != 1.0 && sy != -1.0) {
			cx = matrix(2,2) / cy;
			sx = matrix(2,1) / cy;
			y = ((float)atan2(sx,cx)); // * RAD2DEG ;

			cz = matrix(0,0) / cy;
			sz = matrix(1,0) / cy;
			r = ((float)atan2(sz,cz)); // * RAD2DEG ;
		} else {
			// SINCE Cos(Y) IS 0.  ADOPT THE STANDARD Z = 0

			matrix(1,1) = 1.0 - (2.0 * Q[0] * Q[0]) - (2.0 * Q[2] * Q[2]);
			matrix(1,2) = (2.0 * Q[1] * Q[2]) - (2.0 * Q[3] * Q[0]);

			cx =  matrix(1,1);
			sx = -matrix(1,2);
			y = ((float)atan2(sx,cx)); // * RAD2DEG ;

			cz = 1.0 ;
			sz = 0.0 ;
			r = ((float)atan2(sz,cz)); // * RAD2DEG ;
		}
	}

	Quaternion vec(float x, float y, float z) {
		Quaternion temp;
		temp(0) = x; temp(1) = y; temp(2) = z;
		return temp;
	}

	// propagate quaternion based on msec time and rates
	Quaternion propagate(Matrix &w, float msec ) const {
		float W = w.Norm();
		if(W == 0 || msec == 0)
			return (*this);

		float angle = W*msec/2000.0;
		float c = cos(angle);
		float s = sin(angle);
		Quaternion q;

		q(0) = c*Q[0] + 1/W*s*(              w(2)*Q[1] - w(1)*Q[2] + w(0)*Q[3] );
		q(1) = c*Q[1] + 1/W*s*( -w(2)*Q[0]             + w(0)*Q[2] + w(1)*Q[3] );
		q(2) = c*Q[2] + 1/W*s*(  w(1)*Q[0] - w(0)*Q[1]             + w(2)*Q[3] );
		q(3) = c*Q[3] + 1/W*s*( -w(0)*Q[0] - w(1)*Q[1] + w(2)*Q[2]             );

		return q;
	}

	Quaternion propagate(Quaternion &w, float msec ) const {
		float W = w.norm();
		if(W == 0 || msec == 0)
			return (*this);
		float angle = W*msec/2000.0;
		float c = cos(angle);
		float s = sin(angle);
		Quaternion q;

		q(0) = c*Q[0] + 1/W*s*(              w(2)*Q[1] - w(1)*Q[2] + w(0)*Q[3] );
		q(1) = c*Q[1] + 1/W*s*( -w(2)*Q[0]             + w(0)*Q[2] + w(1)*Q[3] );
		q(2) = c*Q[2] + 1/W*s*(  w(1)*Q[0] - w(0)*Q[1]             + w(2)*Q[3] );
		q(3) = c*Q[3] + 1/W*s*( -w(0)*Q[0] - w(1)*Q[1] - w(2)*Q[2]             );

		return q;
	}


	private:
	float Q[4];
};

const float Quaternion::X_VEC[4] = {1,0,0,0};
const float Quaternion::Y_VEC[4] = {0,1,0,0};
const float Quaternion::Z_VEC[4] = {0,0,1,0};	

#endif //_QUATERNION_H
